#include "MSE_PID.h"
#include "MSE_MOTOR.h"
#include "MSE_DBUS.h"
#include "main.h"
#include "MSE_CAN.h"
/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //头部部分电机结构体
 *	@parameter:		    //
 *	@time:				//22-11-23 19:14
 *	@ReadMe:			//前伸 HORIZONTAL
                        //升降 LIFT
                        //横移 SWAY
 ************************************************************万能分隔符**************************************************************/
// 升降
struct MSE_MOTOR_Typedef MSE_V_MOTOR_LIFT_L = {0}; // 这是个结构体，存储在里面的，不会乱改变
struct MSE_MOTOR_Typedef MSE_V_MOTOR_LIFT_R = {0};
// 前伸
struct MSE_MOTOR_Typedef MSE_V_MOTOR_HORIZONTAL_L = {0};
struct MSE_MOTOR_Typedef MSE_V_MOTOR_HORIZONTAL_R = {0};
// 横移
struct MSE_MOTOR_Typedef MSE_V_MOTOR_SWAY = {0};
// Yaw轴  head
struct MSE_MOTOR_Typedef MSE_V_MOTOR_Yaw = {0};
struct MSE_MOTOR_Typedef MSE_V_MOTOR_L_CUP_PUMP = {0};
struct MSE_MOTOR_Typedef MSE_V_MOTOR_R_CUP_PUMP = {0};
struct MSE_MOTOR_Typedef MSE_V_MOTOR_Pitch = {0};
/**********************************************************************************************************************************************/
struct MSE_MOTOR_Typedef MSE_V_MOTOR_Camera = {0};
// 底盘
struct MSE_MOTOR_Typedef MSE_V_MOTOR_BOTTOM_3508_1 = {0};
struct MSE_MOTOR_Typedef MSE_V_MOTOR_BOTTOM_3508_2 = {0};
struct MSE_MOTOR_Typedef MSE_V_MOTOR_BOTTOM_3508_3 = {0};
struct MSE_MOTOR_Typedef MSE_V_MOTOR_BOTTOM_3508_4 = {0};

/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //3508电机解算函数
 *	@parameter:		    //@电机结构体  @can接收到的数组
 *	@time:				//22-11-23 20:31
 *	@ReadMe:			//底盘运动只要单速度环，所以写了一个没有角度环解算的函数
 ************************************************************万能分隔符**************************************************************/
void MSE_F_MOTOR_CAN_RX_3508RM_WITHOUT_ANGLE(struct MSE_MOTOR_Typedef *MOTOR, uint8_t *can_data)
{
    MOTOR->DATA.Speed_last = MOTOR->DATA.Angle_now;
    MOTOR->DATA.Speed_now = (int16_t)(((can_data[2] << 8) | can_data[3]) & 0xFFFF);
    MOTOR->DATA.ONLINE_JUDGE_TIME = MSE_DF_MOTOR_OFFLINE_TIME;
}

/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //3508电机解算函数
 *	@parameter:		    //@电机结构体  @can接收到的数组
 *	@time:				//22-11-23 18:50
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
void MSE_F_MOTOR_CAN_RX_3508RM(struct MSE_MOTOR_Typedef *MOTOR, uint8_t *can_data)
{
    MOTOR->DATA.Angle_last = MOTOR->DATA.Angle_now;

    MOTOR->DATA.Angle_now = (int16_t)(((can_data[0] << 8) | can_data[1]) & 0xFFFF);

    MOTOR->DATA.Speed_last = MOTOR->DATA.Speed_now;

    MOTOR->DATA.Speed_now = (int16_t)(((can_data[2] << 8) | can_data[3]) & 0xFFFF);

    MOTOR->DATA.current = (int16_t)(((can_data[4] << 8) | can_data[5]) & 0xFFFF);

    MOTOR->DATA.temperature = can_data[6];

    if (MOTOR->DATA.Angle_now - MOTOR->DATA.Angle_last < -4000)
    {
        MOTOR->DATA.Laps++;
    }
    else if (MOTOR->DATA.Angle_now - MOTOR->DATA.Angle_last > 4000)
    {
        MOTOR->DATA.Laps--;
    }

    if ((MOTOR->DATA.Laps > 32500) | (MOTOR->DATA.Laps < -32500))
    {
        MOTOR->DATA.Laps = 0;
        MOTOR->DATA.Aim = MOTOR->DATA.Angle_now;
    }

    MOTOR->DATA.ONLINE_JUDGE_TIME = MSE_DF_MOTOR_OFFLINE_TIME;

    MOTOR->DATA.Angle_Infinite = (int32_t)((MOTOR->DATA.Laps << 13) + MOTOR->DATA.Angle_now);
}

/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //2006电机解算函数
 *	@parameter:		    //@电机结构体  @can接收到的数组
 *	@time:				//22-11-23 18:49
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
void MSE_F_MOTOR_CAN_RX_2006RM(struct MSE_MOTOR_Typedef *MOTOR, uint8_t *can_data)
{   hal++;
    MOTOR->DATA.Angle_last = MOTOR->DATA.Angle_now;

    MOTOR->DATA.Angle_now = (int16_t)(((can_data[0] << 8) | can_data[1]) & 0xFFFF);

    MOTOR->DATA.Speed_last = MOTOR->DATA.Speed_now;

    MOTOR->DATA.Speed_now = (int16_t)(((can_data[2] << 8) | can_data[3]) & 0xFFFF);

    MOTOR->DATA.current = (int16_t)(((can_data[4] << 8) | can_data[5]) & 0xFFFF);

    if (MOTOR->DATA.Angle_now - MOTOR->DATA.Angle_last < -4000)
    {
        MOTOR->DATA.Laps++;
    }
    else if (MOTOR->DATA.Angle_now - MOTOR->DATA.Angle_last > 4000)
    {
        MOTOR->DATA.Laps--;
    }

    if ((MOTOR->DATA.Laps > 32500) | (MOTOR->DATA.Laps < -32500))
    {
        MOTOR->DATA.Laps = 0;
        MOTOR->DATA.Aim = MOTOR->DATA.Angle_now;
    }

    MOTOR->DATA.Angle_Infinite = (int32_t)((MOTOR->DATA.Laps << 13) + MOTOR->DATA.Angle_now);
    MOTOR->DATA.ONLINE_JUDGE_TIME = MSE_DF_MOTOR_OFFLINE_TIME;
}

void WHW_F_MOTOR_CAN_RX_6020RM(struct MSE_MOTOR_Typedef *MOTOR, uint8_t *can_data)
{
    MOTOR->DATA.Angle_last = MOTOR->DATA.Angle_now;

    MOTOR->DATA.Angle_now = (int16_t)(((can_data[0] << 8) | can_data[1]) & 0xFFFF);

    MOTOR->DATA.Speed_last = MOTOR->DATA.Speed_now;

    MOTOR->DATA.Speed_now = (int16_t)(((can_data[2] << 8) | can_data[3]) & 0xFFFF);

    MOTOR->DATA.current = (int16_t)(((can_data[4] << 8) | can_data[5]) & 0xFFFF);

    MOTOR->DATA.temperature = can_data[6];

    //	if(MOTOR->DATA.Angle_now < 2900)	MOTOR->DATA.Laps = 1;
    //	if(MOTOR->DATA.Angle_now > 4000)	MOTOR->DATA.Laps = 0;

    if (MOTOR->DATA.Angle_now - MOTOR->DATA.Angle_last < -4000)
    {
        MOTOR->DATA.Laps++;
    }
    else if (MOTOR->DATA.Angle_now - MOTOR->DATA.Angle_last > 4000)
    {
        MOTOR->DATA.Laps--;
    }

    if ((MOTOR->DATA.Laps > 32500) | (MOTOR->DATA.Laps < -32500))
    {
        MOTOR->DATA.Laps = 0;
        MOTOR->DATA.Aim = MOTOR->DATA.Angle_now;
    }

    MOTOR->DATA.ONLINE_JUDGE_TIME = MSE_DF_MOTOR_OFFLINE_TIME;

    MOTOR->DATA.Angle_Infinite = (int32_t)((MOTOR->DATA.Laps << 13) + MOTOR->DATA.Angle_now);
}

void MSE_F_HEAD_MOTOR_CLEAR(struct MSE_MOTOR_Typedef *MOTOR)
{
      MOTOR->PID_P.I_out = 0.0f;
      MOTOR->PID_S.I_out = 0.0f;
      MOTOR->PID_P.I_Lit = 0;
      MOTOR->PID_S.I_Lit = 0;  
      MSE_V_CONTAL.HEAD.LIFT = MOTOR->DATA.Angle_Infinite;
	  MSE_V_DBUS_STATUS=2;
	
}
// 将本时刻的电机刻度作为初始刻度
void WHW_F_HEAD_MOTOR_SIT(struct MSE_MOTOR_Typedef *MOTOR)
{
    MOTOR->PID_P.I_out = 0.0f;
    MOTOR->PID_S.I_out = 0.0f;
    MOTOR->DATA.Laps = 0;
    MOTOR->DATA.Aim = MOTOR->DATA.Angle_now;

}

void MSE_F_HEAD_MOTOR2006_STUCK(struct MSE_MOTOR_Typedef *MOTOR, uint16_t ERROR_ANGLE, uint16_t ERROR_SPEED, uint16_t ERROR_TIME)
{
    // 一定误差  速度小于某个值初步判断电机卡住，接下来去判断卡住的时间
    if (MSE_F_MATH_ABS_float(MOTOR->PID_P.Error[MSE_DF_NOW]) > ERROR_ANGLE && MSE_F_MATH_ABS_float(MOTOR->DATA.Speed_now) < ERROR_SPEED)
    {

        if (MOTOR->DATA.Stuck_Time > ERROR_TIME)
        {
            MSE_F_HEAD_MOTOR_CLEAR(MOTOR);
            MOTOR->DATA.Stuck_Time = 0;
            MOTOR->DATA.Stuck_Flag[0]++;
        }
    }
    else
    {
        MOTOR->DATA.Stuck_Time = 0;
    }
    MOTOR->DATA.Aim_last = MOTOR->DATA.Aim;
}

void MSE_F_HEAD_MOTOR3508_STUCK(struct MSE_MOTOR_Typedef *MOTOR, uint16_t ERROR_CURRENT, uint16_t ERROR_SPEED)
{

    if (MSE_F_MATH_ABS_float(MOTOR->DATA.Speed_now) < ERROR_SPEED && MSE_F_MATH_ABS_float(MOTOR->DATA.current) > ERROR_CURRENT)
    {
        if (MSE_F_MATH_ABS_float(MOTOR->DATA.current) > ERROR_CURRENT)
        {
            MSE_F_HEAD_MOTOR_CLEAR(MOTOR);
            MOTOR->DATA.Stuck_Flag[0]++;
        }
    }
}

void MSE_F_HEAD_MOTOR6020_STUCK(struct MSE_MOTOR_Typedef *MOTOR, uint16_t ERROR_ANGLE, uint16_t ERROR_SPEED, uint16_t ERROR_TIME)
{
    // 一定误差  速度小于某个值初步判断电机卡住，接下来去判断卡住的时间
    if (MSE_F_MATH_ABS_float(MOTOR->PID_P.Error[MSE_DF_NOW]) > ERROR_ANGLE && MSE_F_MATH_ABS_float(MOTOR->DATA.Speed_now) < ERROR_SPEED)
    {
        MOTOR->DATA.Stuck_Time++;
        if (MOTOR->DATA.Stuck_Time > ERROR_TIME)
        {
            MSE_F_HEAD_MOTOR_CLEAR(MOTOR);
            MOTOR->DATA.Stuck_Time = 0;
            MOTOR->DATA.Stuck_Flag[0]++;
        }
    }
    else
    {
        MOTOR->DATA.Stuck_Time = 0;
    }
    MOTOR->DATA.Aim_last = MOTOR->DATA.Aim;
}

void YAW_MOTOR_HOME(void)
{
	
    static int32_t  init_state=1;
	if(init_state==1)
	{
    first_power =MSE_V_MOTOR_Yaw.DATA.Aim;
	MSE_V_MOTOR_LIFT_L.DATA.Aim=0;
	MSE_V_MOTOR_LIFT_R.DATA.Aim=0;	
	first_left  =MSE_V_MOTOR_LIFT_L.DATA.Aim;
    first_right	=MSE_V_MOTOR_LIFT_R.DATA.Aim;
	init_state=0;	
	}
}
